#include "Physics/IBody.hpp"
#include "Common.hpp"
#include "Math/Math.hpp"
#include "Math/Vector.hpp"
#include "Physics/PhysWorld.hpp"
#include "Utility/InterfaceConversion.hpp"
#include "Utility/Singleton.hpp"

#include "Box2D/Collision/Shapes/b2Shape.h"
#include "Box2D/Collision/Shapes/b2PolygonShape.h"
#include "Box2D/Dynamics/b2Body.h"
#include "Box2D/Dynamics/b2Fixture.h"

namespace Physics
{
	IBody::IBody( Entity::IEntity *setEnt )
		: ent( setEnt ), body( nullptr ), world( Utility::Singleton< PhysWorld >::Get() )
	{
	}

	IBody::~IBody( void )
	{
		if ( GetFixture() )
		{
			body->DestroyFixture( GetFixture() );
		}

		if ( body )
		{
			world->RemoveInternalBody( body );
		}
	}

	void IBody::Initialise( const int bodyType, const int shapeType, const Math::Vectorf &size )
	{
		b2BodyDef bodyDef;
		bodyDef.type = static_cast< b2BodyType >( bodyType );

		body = world->CreateInternalBody( &bodyDef );

		if ( shapeType == PHYS_SHAPE_BOX )
		{
			b2PolygonShape box;
			box.SetAsBox( size.x / 2.0f, size.y / 2.0f );

			body->CreateFixture( &box, 1.0f );
		}
	}

	Entity::IEntity *IBody::GetEntity( void )
	{
		return ent;
	}

	bool IBody::IsAsleep( void ) const
	{
		if ( body )
		{
			return !body->IsAwake();
		}
		else
		{
			return false;
		}
	}

	void IBody::SetAsleep( const bool asleep )
	{
		if ( body )
		{
			body->SetAwake( asleep );
		}
	}

	void IBody::ApplyForce( const Math::Vectorf &force, const Math::Vectorf &pos )
	{
		if ( body )
		{
			body->ApplyForce( Utility::Box2DVector( force ), Utility::Box2DVector( pos ) );
		}
	}

	void IBody::ApplyAngularForce( const float torque )
	{
		if ( body )
		{
			body->ApplyTorque( torque );
		}
	}

	void IBody::ApplyLinearForce( const Math::Vectorf &force )
	{
		if ( body )
		{
			body->ApplyForce( Utility::Box2DVector( force ), body->GetWorldCenter() );
		}
	}

	float IBody::GetAngle( void ) const
	{
		if ( body )
		{
			return Math::RadiansToDegrees( body->GetAngle() );
		}
		else
		{
			return 0.0f;
		}
	}

	void IBody::SetAngle( const float angle )
	{
		if ( body )
		{
			body->SetTransform( body->GetPosition(), Math::DegreesToRadians( angle ) );
		}
	}

	bool IBody::GetRotationFixed( void ) const
	{
		if ( body )
		{
			return body->IsFixedRotation();
		}
		else
		{
			return false;
		}
	}

	void IBody::SetRotationFixed( const bool fixed )
	{
		if ( body )
		{
			body->SetFixedRotation( fixed );
		}
	}

	Math::Vectorf IBody::GetPosition( void ) const
	{
		if ( body )
		{
			return Utility::CaffVector( body->GetPosition() );
		}
		else
		{
			return Math::Vectorf( 0.0f, 0.0f );
		}
	}

	void IBody::SetPosition( const Math::Vectorf &pos )
	{
		if ( body )
		{
			body->SetTransform( Utility::Box2DVector( pos ), body->GetAngle() );
		}
	}

	float IBody::GetDensity( void ) const
	{
		b2Fixture *fixture = GetFixture();

		if ( fixture )
		{
			return fixture->GetDensity();
		}
		else
		{
			return 0.0f;
		}
	}

	void IBody::SetDensity( const float density )
	{
		b2Fixture *fixture = GetFixture();

		if ( fixture )
		{
			fixture->SetDensity( density );
		}
	}

	float IBody::GetFriction( void ) const
	{
		b2Fixture *fixture = GetFixture();

		if ( fixture )
		{
			return fixture->GetFriction();
		}
		else
		{
			return 0.0f;
		}
	}

	void IBody::SetFriction( const float friction )
	{
		b2Fixture *fixture = GetFixture();

		if ( fixture )
		{
			fixture->SetFriction( friction );
		}
	}

	float IBody::GetMass( void ) const
	{
		if ( body )
		{
			return body->GetMass();
		}
		else
		{
			return 0.0f;
		}
	}

	void IBody::SetMass( const float mass )
	{
		if ( body )
		{
			b2MassData data;
			body->GetMassData( &data );

			data.mass = mass;
			body->SetMassData( &data );
		}
	}

	float IBody::GetRestitution( void ) const
	{
		b2Fixture *fixture = GetFixture();

		if ( fixture )
		{
			return fixture->GetRestitution();
		}
		else
		{
			return 0.0f;
		}
	}

	void IBody::SetRestitution( const float restitution )
	{
		b2Fixture *fixture = GetFixture();

		if ( fixture )
		{
			fixture->SetRestitution( restitution );
		}
	}

	Math::Vectorf IBody::GetLocalCentreOfMass( void ) const
	{
		if ( body )
		{
			return Utility::CaffVector( body->GetLocalCenter() );
		}
		else
		{
			return Math::Vectorf( 0.0f, 0.0f );
		}
	}

	Math::Vectorf IBody::GetWorldCentreOfMass( void ) const
	{
		if ( body )
		{
			return Utility::CaffVector( body->GetWorldCenter() );
		}
		else
		{
			return Math::Vectorf( 0.0f, 0.0f );
		}
	}

	float IBody::GetAngularVelocity( void ) const
	{
		if ( body )
		{
			return body->GetAngularVelocity();
		}
		else
		{
			return 0.0f;
		}
	}

	void IBody::SetAngularVelocity( const float angVel )
	{
		if ( body )
		{
			body->SetAngularVelocity( angVel );
		}
	}

	float IBody::GetAngularDamping( void ) const
	{
		if ( body )
		{
			return body->GetAngularDamping();
		}
		else
		{
			return 0.0f;
		}
	}

	void IBody::SetAngularDamping( const float damping )
	{
		if ( body )
		{
			body->SetAngularDamping( damping );
		}
	}

	Math::Vectorf IBody::GetLinearVelocity( void ) const
	{
		if ( body )
		{
			return Utility::CaffVector( body->GetLinearVelocity() );
		}
		else
		{
			return Math::Vectorf( 0.0f, 0.0f );
		}
	}

	void IBody::SetLinearVelocity( const Math::Vectorf &vel )
	{
		if ( body )
		{
			body->SetLinearVelocity( Utility::Box2DVector( vel ) );
		}
	}

	float IBody::GetLinearDamping( void ) const
	{
		if ( body )
		{
			return body->GetLinearDamping();
		}
		else
		{
			return 0.0f;
		}
	}

	void IBody::SetLinearDamping( const float damping )
	{
		if ( body )
		{
			body->SetLinearDamping( damping );
		}
	}

	b2Fixture *IBody::GetFixture( void ) const
	{
		if ( body && body->GetFixtureList() )
		{
			return body->GetFixtureList();
		}
		else
		{
			return nullptr;
		}
	}
}